#!/usr/bin/env python

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped


class PublishMapToLandmarkStaticTF(object):
    """ROS Node class that simply publishes a latched static transformation to 
    /static_transforms for a few seconds. This transformation is between /map 
    and /landmarks, with no rotation or translation. This serves to separate 
    the landmarks from any other part of the map on the TF tree. The node then 
    exits and shuts down.
    """

    def __init__(self):

        # publisher to static_transforms
        pub = rospy.Publisher(
            'static_transforms', 
            TransformStamped,
            queue_size=1,
            latch=True)

        original_frame = rospy.get_param(
            "~original_frame", 
            default="map")
        fixed_frame = rospy.get_param(
            "~fixed_frame", 
            default="landmarks")

        # build identity TF betweeen /orig and /fixed
        tf = TransformStamped()
        tf.header.frame_id = original_frame
        tf.header.stamp = rospy.Time.now()
        tf.child_frame_id = fixed_frame
        tf.transform.translation.x = 0.0
        tf.transform.translation.y = 0.0
        tf.transform.translation.z = 0.0
        tf.transform.rotation.x = 0.0
        tf.transform.rotation.y = 0.0
        tf.transform.rotation.z = 0.0
        tf.transform.rotation.w = 1.0

        # publish tf and log
        pub.publish(tf)
        rospy.loginfo(
            "[tf_map_to_landmark] Broadcasted static transformation between " +
            "\"{}\" and \"{}\".".format(original_frame, fixed_frame))

        rospy.sleep(rospy.Duration(100.0))

        rospy.loginfo("[tf_map_to_landmark] Shutting down...")
        exit()

if __name__=='__main__':
    rospy.init_node('tf_map_to_landmark')
    PublishMapToLandmarkStaticTF()
